#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>

#include "dlist.h"

typedef struct Vertex Vertex;

typedef struct 	Edge {
	Vertex * node;
	int distance;
} Edge;


#define DIRECTIONS 8

struct Vertex{
	char tag;
	//List * edges;
	Edge edges[DIRECTIONS];
	Vertex * prev;
	Vertex * next;
	int distance_form_start;
	int distance_to_target;
};

typedef struct MAP {
	Vertex * nodes;
	int width;
	int height;
} MAP;

void DumpMap(MAP * map)
{
	int x, y;
	for(y = 0; y < map->height; y++) {
		for(x = 0; x < map->width; x++) {
			printf("%c", map->nodes[y*map->width+x].tag);
		}
		printf("\n");
	}
}

Vertex * getNode(MAP * map, int x, int y)
{
	if (x < 0 || x >= map->width ||
		y < 0 || y >= map->height) {
		return 0;
	}
	return &(map->nodes[y*map->width+x]);
}

int PositionOfNode(MAP * map, Vertex * node, int * x, int * y)
{
	int pos = node - map->nodes;
	*y = pos / map->width;
	*x = pos % map->width;
	return 0;
}

MAP * create_map(int width, int height)
{
	MAP * map = malloc(sizeof(MAP));
	map->width = width;
	map->height = height;
	map->nodes = malloc(sizeof(Vertex) * width * height);
	int i, x, y;
	for(y = 0; y < height; y++) {
		for(x = 0; x < width; x++) {
			Vertex * node = getNode(map, x, y);
			if (node->tag == 'x') {
				continue;
			}
			for(i = 0; i < DIRECTIONS; i++) {
				int x_, y_, distance = (i<4)?10:14;
				switch(i) {
					case 0: x_ = x - 1, y_ = y; break;
					case 1: x_ = x + 1, y_ = y; break;
					case 2: x_ = x, y_ = y - 1; break;
					case 3: x_ = x, y_ = y + 1; break;
					case 4: x_ = x + 1, y_ = y + 1; break;
					case 5: x_ = x - 1, y_ = y + 1; break;
					case 6: x_ = x + 1, y_ = y - 1; break;
					case 7: x_ = x - 1, y_ = y - 1; break;
					default:
						continue;
				}
				Vertex * next_node = getNode(map, x_, y_);
				if (next_node && next_node->tag == 'x') {
					next_node = 0;
				}
				node->edges[i].node = next_node;
				node->edges[i].distance  = distance;
				node->tag = '.';
			}
		}
	}
	return map;
}

typedef struct TreeNode {
	void * data;

	struct TreeNode * parent;
	struct TreeNode * child;

	struct TreeNode * prev;
	struct TreeNode * next;
} TreeNode;

typedef struct Tree{
	TreeNode * node;
} Tree;

void TreeRemove(Tree * tree, TreeNode * node)
{
	if (node == 0) {
		return;
	}

	if (node == tree->node) {
		tree->node = 0;
	} else {
		TreeNode * parent = node->parent;
		dlist_remove(parent->child, node);
	}
}

TreeNode * TreeInsert(TreeNode * parent, void * data)
{
	TreeNode * node = malloc(sizeof(TreeNode));
	node->data = data;
	node->parent = parent;
	node->child = 0;
	node->next = 0;
	node->prev = 0;

	if (parent) {
		dlist_insert_tail(parent->child, node);
	}
	return node;
}

TreeNode * TreeNodeFind(TreeNode * root, void * data)
{
	if (root->data == data) {
		return root;
	}

		TreeNode * next = 0;
	TreeNode * child = root->child;
	while((next = dlist_next(child, next))) {
		TreeNode * ret = TreeNodeFind(next, data);
		if (ret) { return ret; }
	}
	return 0;
}

TreeNode * TreeFind(Tree * root, void * data)
{
	return TreeNodeFind(root->node, data);
}

#define VERY_FAST  9999999

int Distance(Vertex * node1, Vertex * node2)
{
	int i;
	for(i = 0; i < DIRECTIONS; i++) {
		if (node1->edges[i].node == node2) {
			return node1->edges[i].distance;
		}
	}
	return VERY_FAST;
}

const char * node_position(MAP * map, Vertex* node)
{
	static char buff[1024] = {0};
	int pos = node - map->nodes;
	int x = pos % map->width;
	int y = pos / map->width;
	sprintf(buff, "%d,%d", x, y);
	return buff;
}

static int debug = 0;

Vertex * dijkstra(MAP * map, Vertex * from, Vertex * to)
{
	Vertex * path = 0;

	Vertex * U = 0;

	int target_pos = to - map->nodes;
	int target_x = target_pos % map->width;
	int target_y = target_pos / map->width;

	int x, y;
	for(y = 0; y < map->height; y++) {
		for(x = 0; x < map->width; x++) {
			Vertex * node = getNode(map, x, y);
			if (node->tag == 'x') continue;
			node->distance_form_start = VERY_FAST;
			int offset_x = target_x - x;
			int offset_y = target_y - y;
			offset_y = (offset_y<0) ? - offset_y : offset_y;
			offset_x = (offset_x<0) ? - offset_x : offset_x;
			node->distance_to_target =  (offset_x + offset_y) * 10;
			dlist_insert_tail(U, node);
		}
	}


	Tree pathTree;
	dlist_remove(U, from);

	pathTree.node = TreeInsert(0, from);
	from->distance_form_start = 0;
	from->next = from->prev = 0;

	if (debug) {
		printf("use node %s as next check pos\n", node_position(map, from));
	}

	TreeNode * cur_node = pathTree.node;
	while(U) {
		Vertex * v = 0;
		Vertex * min_v = 0;
		Vertex * cur_v = cur_node->data;
		int cur_dist = cur_v->distance_form_start;

		int min_dist = VERY_FAST;
		int min_to_target = VERY_FAST;
		while((v = dlist_next(U, v))) {
			int edge_dist = Distance(cur_v, v);
			if (edge_dist != VERY_FAST) {
				if (cur_dist + edge_dist < v->distance_form_start) {
					v->distance_form_start = cur_dist + edge_dist;
					TreeNode * n = TreeFind(&pathTree, v);
					if (n) {
						if (debug) {
							printf("update distance of node %s is %d + %d\n", node_position(map, v),
									v->distance_form_start, v->distance_to_target);
						}
						TreeRemove(&pathTree, n);
					} else {
						if (debug) {
							printf("set distance of node %s is %d + %d\n", node_position(map, v),
									v->distance_form_start, v->distance_to_target);
						}
					}
					TreeInsert(cur_node, v);
					v->tag = 'o';
				}
			}

			if (v->distance_form_start + v->distance_to_target < min_dist) {
				min_to_target = v->distance_to_target;
				min_dist = v->distance_form_start + v->distance_to_target;
				min_v = v;
			}
#if 0
			else if (v->distance_form_start + v->distance_to_target == min_dist) {
				if (v->distance_to_target < min_to_target) {
					printf("!!! use node %s by min_to_target %d/%d\n", node_position(map, v),
							v->distance_to_target, min_dist);
					min_to_target = v->distance_to_target;
					min_v = v;
				}
			}
#endif
		}

		if (min_v == 0) {
			break;
		}

		cur_v->tag = '+';
		dlist_remove(U, min_v);
		cur_node = TreeFind(&pathTree, min_v);
		min_v->tag = '@';

		if (min_v == to) {
			break;
		}

		if (debug) {
			printf("use node %s as next check pos %d + %d\n", node_position(map, min_v),
				min_v->distance_form_start, min_v->distance_to_target);
			DumpMap(map);
			getchar();
		}
	}

	TreeNode * node = TreeFind(&pathTree, to);

	while(node) {
		Vertex * v = node->data;
		v->next = path;
		path = v;
		node = node->parent;
	}

	return path;
}

int main(int argc, char * argv[])
{
	if (argc > 1 && strcmp(argv[1], "-d") == 0) {
		debug = 1;
	}

	srand(time(0));

	MAP * m = create_map(50,10);

#if 1
	int i;
	for(i = 0; i < 10; i++) {
		if (i == 5) continue;
		Vertex * node = getNode(m, 25, i);
		node->tag = 'x';
	}

#endif

	DumpMap(m);
	Vertex * from = getNode(m, 0, 0);
	Vertex * to = getNode(m, 49, 9);

	Vertex * path = dijkstra(m, from, to);
	printf("----\n");

	if (path == 0) {
		printf("no found\n");
		return -1;
	}

	while(path) {
		path->tag = '*';
		path = path->next;	
	}
	DumpMap(m);
	return 0;
}
